﻿using System;
using System.Collections.Generic;
using System.Text;
using RoverStudio.Common.PositionSource;
using RoverStudio.Common.WorldModel;
using RoverStudio.Math;

namespace CaterpillarRover
{
    public class ObjectFollowingController
    {
        private TankHighLevelController highLevelController;
        private IPositionSource objectToFollow;
        private WorldModel worldModel;
        private Path path;
        private double minimumWaypointDistance = 0.35;
        private Vector3D lastPosition;
        private Vector3D lastObjectPosition;

        enum ObjectFollowerState
        {
            Stopped = 0,
            Logging = 1,
            Following = 2
        }
        private ObjectFollowerState state  = ObjectFollowerState.Stopped;

        public double MinimumWaypointDistance
        {
            get { return minimumWaypointDistance; }
            set { minimumWaypointDistance = value; }
        }

        public TankHighLevelController HighLevelController
        {
            get { return highLevelController; }
            set { highLevelController = value; }
        }

        public IPositionSource ObjectToFollow
        {
            set
            {
                if (objectToFollow != null)
                    objectToFollow.PositionUpdated -= new PositionUpdatedHandler(objectToFollow_PositionUpdated);
                objectToFollow = value;
                if (objectToFollow != null)
                    objectToFollow.PositionUpdated += new PositionUpdatedHandler(objectToFollow_PositionUpdated);
            }
            get
            {
                return objectToFollow;
            }
        }


        public ObjectFollowingController(WorldModel worldModel, IPositionSource objectToFollow, TankHighLevelController highLevelController)
        {
            this.worldModel = worldModel;
            this.objectToFollow = objectToFollow;
            this.highLevelController = highLevelController;
            objectToFollow.PositionUpdated += new PositionUpdatedHandler(objectToFollow_PositionUpdated);
            highLevelController.PositionSource.PositionUpdated += new PositionUpdatedHandler(SelfPositionSource_PositionUpdated);
            path = new Path(worldModel);
        }

        void SelfPositionSource_PositionUpdated(Vector3D position, Rotation orientation, DateTime time)
        {
            lastPosition = position;
        }

        void objectToFollow_PositionUpdated(RoverStudio.Math.Vector3D position, RoverStudio.Math.Rotation orientation, DateTime time)
        {
            lastObjectPosition = position;
            if (path.PathElements.Count == 0)
                path.PathElements.Add(position);
            else
            {
                double distanceFromLast = (position - path.PathElements[path.PathElements.Count - 1]).Length2D();
                if (distanceFromLast > minimumWaypointDistance)
                    path.PathElements.Add(position);
            }
        }

        public void StartLogging()
        {
            state |= ObjectFollowerState.Logging;
        }

        public void StopLogging()
        {
            state &= ~ObjectFollowerState.Logging;
            
        }

        public void ClearHistoricalPath()
        {
            path.CreateNewPath();
        }

        public void StartFollowing()
        {
            path.PathElements.Insert(0, lastPosition); // start at current position
            HighLevelController.StartPathFollowing(path);
        }



    }
}
